#include "mission.h"
uint8_t mission_num = 1; // 任务题号
uint8_t N_round = 1;     // 圈数
uint8_t car_mode = 0;    // 用于状态机切换
uint8_t is_using_Yaw = 0;
uint8_t Mission_Start_Flag = 0;
int Encoder_Start = 0; // 起始编码器值
int Encoder_End = 0;
float Angle_Start_Yaw = 0;
float Angle_End_Yaw = 0;
uint8_t turn_flag = 0; // 0: 左转电机, 1: 右转电机
uint8_t Mission3_Trigger = 0;   //使能的时候 mission3proc开始调用
#define STEPPER_ZOOM 0.01f

#define STOP_TICK 150  // 停止时间，单位ms
#define GO_TICK   100  // 运动时间，单位ms

void mission3_proc(void)   // 10ms周期调用
{
    if (Mission3_Trigger == 0) return; // 没有使能时不执行

    static uint8_t running = 1;          // 1:运动, 0:停止
    static uint32_t last_tick = 0;

    uint32_t now_tick = get_tick();

    if (running)
    {
        // 电机运动
        if (turn_flag)
            StepperMotor_SetSpeed(&stepper_motor_A, 5.0f, 0);
        else
            StepperMotor_SetSpeed(&stepper_motor_A, -5.0f, 0);

        // 判断是否到达运动时长
        if (now_tick - last_tick >= GO_TICK)
        {
            running = 0;            // 进入停止状态
            last_tick = now_tick;   // 更新时间戳
        }
    }
    else
    {
        // 电机停止
        StepperMotor_SetSpeed(&stepper_motor_A, 0.0f, 0);

        // 判断是否到达停止时长
        if (now_tick - last_tick >= STOP_TICK)
        {
            running = 1;            // 进入运动状态
            last_tick = now_tick;   // 更新时间戳
        }
    }
}








/**
 * @brief 计算步进电机从当前位置旋转到目标Yaw角最短路径的步数
 * @param current_yaw 当前Yaw角, 单位: 度，范围[-180, 180]
 * @param target_yaw  目标Yaw角, 单位: 度，范围[-180, 180]
 * @return 步进电机需要转动的步数，正值为顺时针，负值为逆时针
 */
int YawToStep(float current_yaw, float target_yaw)
{
    // 步进电机参数
    const float step_angle = 1.8f;                          // 每步角度
    const int micro_steps = 16;                             // 细分
    const float real_step_angle = step_angle / micro_steps; // 实际每步角度

    // 计算目标与当前角度差
    float delta = target_yaw - current_yaw;

    // 归一化到 [-180, 180]
    while (delta > 180.0f)
        delta -= 360.0f;
    while (delta < -180.0f)
        delta += 360.0f;

    // 若顺时针和逆时针都可到达，选最近的方向
    // delta为正表示顺时针，负为逆时针
    int steps = (int)(delta / real_step_angle + (delta > 0 ? 0.5f : -0.5f)); // 四舍五入

    return steps;
}

void Mission_E(void)
{
    if (mission_num == 1 && Mission_Start_Flag) // 绕黑线行驶 圈数为N
    {
        if (N_round)
        {
            uint8_t BL_Sum = GW_Check_Sum();
            if (BL_Sum > 3 || is_using_Yaw) // 向左直角转弯
            {

                if (is_using_Yaw == 0)
                {
                    Angle_End_Yaw = Normal_Yaw(wit_data.yaw + 90.0f);
                    is_using_Yaw = 1;
                }

                // 转向环
                float control_val = PID_Calc(&Yaw_PID, Angle_End_Yaw, wit_data.yaw, 1);
                my_set_speed(BASE_SPEED - control_val, BASE_SPEED + control_val);
                if (wit_data.yaw >= Angle_End_Yaw - 3.0f && wit_data.yaw <= Angle_End_Yaw + 3.0f)
                {
                    is_using_Yaw = 0;
                }
            }
            else
            {
                float zoom = BASE_SPEED / 300.0f;
                float control_val = PID_Calc(&GW_gray_PID, 0, gw_offset, 0);
                my_set_speed(BASE_SPEED - control_val * zoom, BASE_SPEED + control_val * zoom);
            }
            if (Encoder_Now[0] + Encoder_Now[3] >= Encoder_End) // 到达终点
            {
                my_set_speed(0, 0);     // 停止
                Mission_Start_Flag = 0; // 重置任务开始标志
            }
        }
    }
    // else if (mission_num == 2 && Mission_Start_Flag) // 控制舵机云台使得趋向于x,y
    // {
    //     static uint8_t servo1_lock = 0;
    //     static uint8_t servo3_lock = 0;

    //     // Dx Servo1 越往左,角度越大 值减小         // Dy Servo3 越往下,角度越大 值减小
    //     if (is_findblob && abs(XY_Servo.x_delta) <= MID_DELTA && abs(XY_Servo.y_delta) <= MID_DELTA)
    //     {
    //         // 达到宽容度范围，上锁
    //         servo1_lock = 1;
    //         servo3_lock = 1;
    //         Mission_Start_Flag = 0;
    //         set_servo1_angle(XY_Servo.x_angle); // 设置舵机1角度
    //         set_servo3_angle(XY_Servo.y_angle); // 设置舵机3角度
    //     }
    //     else
    //     {
    //         // 只要有超出宽容度，解锁
    //         if (abs(XY_Servo.x_delta) > MID_DELTA)
    //             servo1_lock = 0;
    //         if (abs(XY_Servo.y_delta) > MID_DELTA)
    //             servo3_lock = 0;

    //         // x、y都超出宽容度
    //         if (is_findblob && abs(XY_Servo.x_delta) > MID_DELTA && abs(XY_Servo.y_delta) > MID_DELTA)
    //         {
    //             if (!servo1_lock)
    //             {
    //                 float control_val_x = PID_Calc(&Servox_PID, 0, XY_Servo.x_delta, 0);
    //                 XY_Servo.x_angle -= control_val_x * 0.1f; // 调整x轴角度
    //                 set_servo1_angle(XY_Servo.x_angle);
    //             }
    //             if (!servo3_lock)
    //             {
    //                 float control_val_y = PID_Calc(&Servoy_PID, 0, XY_Servo.y_delta, 0);
    //                 XY_Servo.y_angle += control_val_y * 0.1f; // 调整y轴角度
    //                 set_servo3_angle(XY_Servo.y_angle);
    //             }
    //         }
    //         // 只y超出
    //         else if (is_findblob && abs(XY_Servo.x_delta) <= MID_DELTA && abs(XY_Servo.y_delta) > MID_DELTA)
    //         {
    //             servo1_lock = 1; // x已达标，上锁
    //             if (!servo3_lock)
    //             {
    //                 float control_val_y = PID_Calc(&Servoy_PID, 0, XY_Servo.y_delta, 0);
    //                 XY_Servo.y_angle += control_val_y * 0.1f;
    //                 set_servo3_angle(XY_Servo.y_angle);
    //             }
    //         }
    //         // 只x超出
    //         else if (is_findblob && abs(XY_Servo.x_delta) > MID_DELTA && abs(XY_Servo.y_delta) <= MID_DELTA)
    //         {
    //             servo3_lock = 1; // y已达标，上锁
    //             if (!servo1_lock)
    //             {
    //                 float control_val_x = PID_Calc(&Servox_PID, 0, XY_Servo.x_delta, 0);
    //                 XY_Servo.x_angle -= control_val_x * 0.1f;
    //                 set_servo1_angle(XY_Servo.x_angle);
    //             }
    //         }
    //     }
    // }
    else if (mission_num == 2 && Mission_Start_Flag) // 控制步进电机云台使得趋向于x,y
    {
        static uint8_t StepperA_lock = 0;
        static uint8_t StepperB_lock = 0;
        static uint8_t Stepper_init = 0; // 使得步进电机正向运动540步后开始真正的控制
        if (Stepper_init == 0 && is_findblob)
        {
            // 直接开始
            Stepper_init = 1; // 步进电机正向运动540步后开始真正的控制
        }
        if (Stepper_init == 0 && StepperCnt_buff[1] > -540)
        {
            StepperMotor_SetSpeed(&stepper_motor_B, -0.5f,0); // 反向运动
        }
        else if (Stepper_init == 0 && StepperCnt_buff[1] <= -540)
        {
            Stepper_init = 1;                              // 步进电机B正向运动540步后开始真正的控制
            StepperMotor_SetSpeed(&stepper_motor_B, 0.0f,0); // 停止B
        }
        if (Stepper_init)
        {
            if (!is_findblob)
            {
                StepperMotor_SetSpeed(&stepper_motor_A, 0.0f,0);
                StepperMotor_SetSpeed(&stepper_motor_B, 0.0f,0);
                return;
            }
            // Dx Stepper_motor_A 越往左,角度越大 值减小         // Dy Stepper_motor_B 越往下,角度越大 值减小
            if (is_findblob && abs(XY_Servo.x_delta) <= MID_DELTA && abs(XY_Servo.y_delta) <= MID_DELTA)
            {
                // 达到宽容度范围，上锁
                StepperA_lock = 1;
                StepperB_lock = 1;
                Mission_Start_Flag = 0;
                StepperMotor_SetSpeed(&stepper_motor_A, 0.0f,0); // 设置步进电机A速度为0
                StepperMotor_SetSpeed(&stepper_motor_B, 0.0f,0); // 设置步进电机B速度为0
                Relay_on();                                    // 打开继电器
            }
            else
            {
                // 只要有超出宽容度，解锁
                if (abs(XY_Servo.x_delta) > MID_DELTA)
                    StepperA_lock = 0;
                if (abs(XY_Servo.y_delta) > MID_DELTA)
                    StepperB_lock = 0;

                // x、y都超出宽容度
                if (is_findblob && abs(XY_Servo.x_delta) > MID_DELTA && abs(XY_Servo.y_delta) > MID_DELTA)
                {
                    if (!StepperA_lock)
                    {
                        float control_val_x = PID_Calc(&Servox_PID, 0, XY_Servo.x_delta, 0);
                        StepperMotor_SetSpeed(&stepper_motor_A, STEPPER_ZOOM * control_val_x,0);
                    }
                    if (!StepperB_lock)
                    {
                        float control_val_y = -PID_Calc(&Servoy_PID, 0, XY_Servo.y_delta, 0);
                        StepperMotor_SetSpeed(&stepper_motor_B, STEPPER_ZOOM * control_val_y,0);
                    }
                }
                // 只y超出
                else if (is_findblob && abs(XY_Servo.x_delta) <= MID_DELTA && abs(XY_Servo.y_delta) > MID_DELTA)
                {
                    StepperA_lock = 1; // x已达标，上锁
                    if (!StepperB_lock)
                    {
                        float control_val_y = -PID_Calc(&Servoy_PID, 0, XY_Servo.y_delta, 0);
                        StepperMotor_SetSpeed(&stepper_motor_B, STEPPER_ZOOM * control_val_y,0);
                    }
                }
                // 只x超出
                else if (is_findblob && abs(XY_Servo.x_delta) > MID_DELTA && abs(XY_Servo.y_delta) <= MID_DELTA)
                {
                    StepperB_lock = 1; // y已达标，上锁
                    if (!StepperA_lock)
                    {
                        float control_val_x = PID_Calc(&Servox_PID, 0, XY_Servo.x_delta, 0);
                        StepperMotor_SetSpeed(&stepper_motor_A, STEPPER_ZOOM * control_val_x,0);
                    }
                }
            }
        }
    }
    else if (mission_num == 3 && Mission_Start_Flag) // 控制步进电机云台使得趋向于x,y .使用jy62快速定位到大致位置后对准矩形
    {
        if (!is_findblob)
        {
            Mission3_Trigger = 1; // 使能mission3_proc

            return;
        }
        else{
            Mission3_Trigger = 0; // 禁用mission3_proc
            mission_num = 2;

            return;
        }
            
        
        // 如果找到blobs则直接瞄准，如果没查找到就向目标位置移动
    }
}